/**
  ****************************RM Warrior 2023****************************
  * @file       motor_task.c/h
  * @brief      对底盘前进速度和旋转速度控制
	*             电机速度PI环：500HZ
	*             
	*             
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     2023/2/         pxx              ......
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************RM Warrior 2023****************************
  */

#include "motor_task.h"
#include "main.h"
#include "lcd_key_led.h"
#include "stdio.h"
#include "step_motor_usart.h"

#include "FreeRTOSConfig.h"
#include "FreeRTOS.h"
#include "task.h"

#define   STEP_CONTROL_USART     0



#if INCLUDE_uxTaskGetStackHighWaterMark
uint32_t MotorTaskStack;
#endif
static Real_Data  yaw_data;
static Real_Data  pitch_data;

void motor_task(void *pvParameters)
{
	
	static int T_Cnt=0;
	
	static int i=1;
	static int Pitch_cnt=0;
	static int Yaw_cnt=0;
	
	static fp32 Pitch_cnt_vice=0;
	static fp32 Yaw_cnt_vice=0;
	
	static int T_Pitch_Cnt=0;
	static int T_Yaw_Cnt=0;
	
	const Gimbal_Motor_t* pitch_motor;
	const Gimbal_Motor_t* yaw_motor;
	
	pitch_motor = get_pitch_motor_point();
	yaw_motor = get_yaw_motor_point();
	
	fp32 pitch_vice_set;
	fp32 yaw_vice_set;
	
//	fp32 motor_angle = 0;
//	fp32 motor_gyro = 0;

#if !STEP_CONTROL_USART
	if(!STEP_CONTROL_USART)   {
//		step_motor_msg.data = get_data_point();
		if(STEP_MOTOR_1_EN)                                 //电机1使能
			EN1_H;
		else
			EN1_L;
		
		if(STEP_MOTOR_2_EN)                                 //电机2使能
			EN2_H;
		else
			EN2_L;
	}
	
#else

		
		
		

#endif
	
    while (1)
    {
		
#if !STEP_CONTROL_USART
			
			
		
//			pitch_vice_set = (pitch_motor->given_current)*RAD_TO_ANGLE;       //角速度 度每秒
//			yaw_vice_set = (yaw_motor->given_current)*RAD_TO_ANGLE;
//			
//			if(pitch_vice_set>0) {
//				DIR1_L;
//				Pitch_cnt_vice = pitch_vice_set/STEP_ANGLE;                        //cnt速度，toggle/s
//				Pitch_cnt = (1000/(int)Pitch_cnt_vice);
//				if(Pitch_cnt<1)
//					Pitch_cnt = 1;
//				i=20;
//				while(i>0) {
//					i--;
//					
//				}
//				T_Pitch_Cnt ++;
//				T_Pitch_Cnt%=Pitch_cnt;
//				if(T_Pitch_Cnt==0)
//					STEP1_TOGGLE;
//			}
//			else if(pitch_vice_set<0){
//				DIR1_H;
//				Pitch_cnt_vice = -pitch_vice_set/STEP_ANGLE;                        //cnt速度，toggle/s
//				Pitch_cnt = (1000/(int)Pitch_cnt_vice);
//				if(Pitch_cnt<1)
//					Pitch_cnt = 1;
//				T_Pitch_Cnt ++;
//				T_Pitch_Cnt%=Pitch_cnt;
//				if(T_Pitch_Cnt==0)
//					STEP1_TOGGLE;
//			}
//			else
//			{
//				Pitch_cnt = 0;
//				T_Pitch_Cnt = 0;
//			}
//			
//			if(yaw_vice_set>0) {
//				DIR2_L;
//				Yaw_cnt_vice = yaw_vice_set/STEP_ANGLE;                        //cnt速度，toggle/s
//				Yaw_cnt = (1000/(int)Yaw_cnt_vice);
//				if(Yaw_cnt<1)
//					Yaw_cnt = 1;
//				T_Yaw_Cnt ++;
//				T_Yaw_Cnt%=Yaw_cnt;
//				if(T_Yaw_Cnt==0)
//					STEP2_TOGGLE;
//			}
//			else if(yaw_vice_set<0){
//				DIR2_H;
//				Yaw_cnt_vice = -yaw_vice_set/STEP_ANGLE;                        //cnt速度，toggle/s
//				
//				Yaw_cnt = (1000/(int)Yaw_cnt_vice);
//				if(Yaw_cnt<1)
//					Yaw_cnt = 1;
//				T_Yaw_Cnt ++;
//				T_Yaw_Cnt%=Yaw_cnt;
//				if(T_Yaw_Cnt==0)
//					STEP2_TOGGLE;
//			}
//			else
//			{
//				Yaw_cnt = 0;
//				T_Yaw_Cnt = 0;
			}
			
			
			
		
#else
		
	
			
//			if(T_Cnt==0)
//				get_pitch_data();
			if(T_Cnt==0) {
				
//				motor_angle = yaw_motor->gimbal_motor_measure->ecd*Motor_Ecd_to_Rad;
//				motor_gyro = ( yaw_motor->gimbal_motor_measure->ecd -  yaw_motor->gimbal_motor_measure->last_ecd)*Motor_Ecd_to_Rad*250;
//				
//				motor_angle = motor_angle*MOTOR_RAD_TO_REAL_RAD;
//				motor_gyro = motor_gyro*MOTOR_RAD_TO_REAL_RAD;
				
				
//				if(motor_angle<=10&&motor_angle>=-10) {
//					yaw_data.relative_angle = motor_angle;
//					yaw_data.motor_gyro = motor_gyro;
//				}
				if(pitch_motor->given_current >= 0) {
					pitch_vice_set = (int)((pitch_motor->given_current)*PITCH_DANGWEI_TO_REAL_GYRO);
					set_pitch_vice(0, pitch_vice_set);
				}
					
				else {
					pitch_vice_set = -(int)((pitch_motor->given_current)*PITCH_DANGWEI_TO_REAL_GYRO);
					set_pitch_vice(1, pitch_vice_set);
				}

			}
//			else if(T_Cnt==2)
//				get_yaw_data();
			else if(T_Cnt==1) {
//				motor_angle = pitch_motor->gimbal_motor_measure->ecd*Motor_Ecd_to_Rad;
//				motor_gyro = ( pitch_motor->gimbal_motor_measure->ecd -  pitch_motor->gimbal_motor_measure->last_ecd)*Motor_Ecd_to_Rad*250;
//				
//				motor_angle = motor_angle*MOTOR_RAD_TO_REAL_RAD;
//				motor_gyro = motor_gyro*MOTOR_RAD_TO_REAL_RAD;
//				
//				if(motor_angle<=10&&motor_angle>=-10) {
//					pitch_data.relative_angle = motor_angle;
//					pitch_data.motor_gyro = motor_gyro;
//				}
				
//				printf("%d, %d, %d, %d, %f\n", \
//				pitch_data->ecd,\
//				pitch_data->last_ecd,\
//				1,          \
//				1,         \
//				pitch_motor->relative_angle);
				
				
				if(yaw_motor->given_current >= 0) {
					yaw_vice_set = (int)((yaw_motor->given_current)*YAW_DANGWEI_TO_REAL_GYRO);
					set_yaw_vice(0, yaw_vice_set);
				}
					
				else {
					yaw_vice_set = -(int)((yaw_motor->given_current)*YAW_DANGWEI_TO_REAL_GYRO);
					set_yaw_vice(1, yaw_vice_set);
				}

			}
				
			
			
			T_Cnt ++;
			T_Cnt=T_Cnt%2;
				
	
#endif

			
			vTaskDelay(STEP_SPEED);
#if INCLUDE_uxTaskGetStackHighWaterMark
        MotorTaskStack = uxTaskGetStackHighWaterMark(NULL);
#endif
    }
}



const Real_Data* get_pitch_real_point(void)
{
	return &pitch_data;
	
}

const Real_Data* get_yaw_real_point(void)
{
	return &yaw_data;
	
}




